/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file fpManager.cxx
  \author Cory Dixon
*/

#include <stdlib.h>
#include <stdio.h>

#include "distMeasure.h"
#include "vcConfig.h"
#include "color.h"

extern GuiStruct guiStr;

/*<-End Global Variables-->*/

DMManager::DMManager() : LayerManager()
{
	dmLines.plan.clear_list();

	for( int j = 0; j < NUM_WP_POINTS; j++) {
		dmLines.imgPoints[j].line_next = NULL;
		for( int k = 0; k < MAX_PREV_LINE; k++)
			dmLines.imgPoints[j].line_prev[k] = NULL;
	}

	pointWidth = 5.0;
	lineWidth  = 2.0;

	drag_point    = -1;
	dragging      = false;
	creating_plan = false;
	last_theta    = NAN;
}

DMManager::~DMManager()
{
}

void DMManager::init(GPSMap *map)
{
	LayerManager::init(map);
	layerG = GOO_CANVAS_ITEM(gps_map->getLayerG(GPSMap::DM_LAYER));
}

void DMManager::updateMap()
{
	//cout << "DMManager::updateMap()" << endl;
	return;
}

void DMManager::translate_from_zoomChange(double zoom_factor)
{
	GPSWaypoint_t gpsPt;
	double x,y,w,h;
	GooCanvasPoints *points = NULL;

	//cout << "DMManager::translate_from_zoomChange - zoom_factor=" << zoom_factor << endl;

	//--  loop through all clients flight plans --//
		//--  loop through and convert all lat/lon points to image within flightplans --//
		for( int wpInd = 0; wpInd < NUM_WP_POINTS; wpInd++ ) {
			if( dmLines.imgPoints[wpInd].pointG ) {
				// get lat/lon coordinates
				gpsPt = dmLines.plan.get_waypoint(wpInd);
				x = gpsPt.Lon;
				y = gpsPt.Lat;

				// transform into window
				gps_map->geoToLayer(GPSMap::DM_LAYER, x, y);

				// save to array
				dmLines.imgPoints[wpInd].x = x;
				dmLines.imgPoints[wpInd].y = y;

				g_object_set (dmLines.imgPoints[wpInd].pointG, "x", x, "y", y, NULL);

				if( dmLines.imgPoints[wpInd].orbit ) {
					g_object_get(dmLines.imgPoints[wpInd].orbit, 
							"radius-x", &w, "radius-y", &h, NULL);
					g_object_set(dmLines.imgPoints[wpInd].orbit, 
							"radius-x", w * zoom_factor, "radius-y", h * zoom_factor, NULL);
				}


				if( dmLines.imgPoints[wpInd].line_next ) {
					points = NULL;
					g_object_get (dmLines.imgPoints[wpInd].line_next, "points", &points, NULL);
					if( points ) {
						points->coords[0] = x;
						points->coords[1] = y;
						g_object_set (dmLines.imgPoints[wpInd].line_next, "points", points, NULL);
						goo_canvas_points_unref(points);
					}
				}

				for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++) {
					points = NULL;
					if( dmLines.imgPoints[wpInd].line_prev[p_ind] ) {
						g_object_get (dmLines.imgPoints[wpInd].line_prev[p_ind], "points", &points, NULL);
						if( points ) {
							points->coords[2] = x;
							points->coords[3] = y;
							g_object_set (dmLines.imgPoints[wpInd].line_prev[p_ind], "points", points, NULL);
							goo_canvas_points_unref(points);
						}
					}
				}
			}
		}
}

void DMManager::create_canvas_flightplan()
{
	GPSWaypoint_t gpsPt;
	double x,y;

	//cout << "DMManager::create_canvas_flightplan - clientInd=" << clientInd << " fpG=" << layerG << endl;

	//--  loop through and convert all lat/lon points to image --//
	for( int i = 0; i < NUM_WP_POINTS; i++ ) {
		// get lat/lon coordinates
		gpsPt = dmLines.plan.get_waypoint(i);
		x = gpsPt.Lon;
		y = gpsPt.Lat;

		// transform into window
		gps_map->geoToLayer(GPSMap::DM_LAYER, x, y);

		// save to array
		dmLines.imgPoints[i].x = x;
		dmLines.imgPoints[i].y = y;

	}

	String stroke_color = "green";

	// Create group to hold enture flight plan
	if( dmLines.wp_group == NULL) 
	{
		dmLines.wp_group = goo_canvas_group_new (GOO_CANVAS_ITEM(layerG), NULL);

		String nameStr = "Local";

		// make sure layer is shown
		gps_map->addLayer( nameStr, dmLines.wp_group, (GooCanvasItem*)GPSMap::DM_LAYER);
		gps_map->setLayerListShown( dmLines.wp_group, 1);

		// make sure parent manager layer is shown
		gps_map->toggleLayerShown(GPSMap::DM_LAYER, true);
	}

	// loop through and draw the connectors for the flight plans 
	WPMask_t mask, plan_mask;
	SetAllWPMaskBits(mask);

	double total_range = 0;
	int32_t i = FindFirstWPMaskBitSet(mask);
	while(i >= 0)
	{
		// is this point a member of a waypoint plan
		ClearAllWPMaskBits(plan_mask);
		int numPoints = dmLines.plan.GetPointsInPlan(plan_mask, i);
		if( numPoints > 0 )
		{
			// now loop through plan and draw lines
			int32_t j = FindFirstWPMaskBitSet(plan_mask);

			while(j >= 0)
			{
				gpsPt = dmLines.plan.get_waypoint(j);
				int ind = gpsPt.index;
				if( ind < NUM_WP_POINTS && gpsPt.IsPointValid() ) {
					int thisInd = gpsPt.index;
					int nextInd = gpsPt.Next;

					ClearWPMaskBit(dmLines.editing_mask, thisInd);

					// get next waypoint
					GPSWaypoint_t gpsPt_next = dmLines.plan.get_waypoint(nextInd);

					//-- draw line to next next waypoint --//
					if( gpsPt_next.index == nextInd && gpsPt_next.IsPointValid() )
					{
						GPSLLA_t gps1, gps2;
						gps1.lat = gpsPt.Lat;
						gps1.lon = gpsPt.Lon;
						gps1.alt = gpsPt.Alt;

						gps2.lat = gpsPt_next.Lat;
						gps2.lon = gpsPt_next.Lon;
						gps2.alt = gpsPt_next.Alt;

						// calculate distance
						double range = GPSRange(gps1, gps2);
						total_range += range;
						//cout << "Range to Next: " << range << endl;
						dmLines.imgPoints[nextInd].distPrev  = range;
						dmLines.imgPoints[nextInd].distTotal = total_range;

						dmLines.imgPoints[thisInd].line_next = goo_canvas_polyline_new_line( dmLines.wp_group,
								dmLines.imgPoints[thisInd].x, dmLines.imgPoints[thisInd].y,
								dmLines.imgPoints[nextInd].x, dmLines.imgPoints[nextInd].y,
								"line-width", lineWidth,
								"stroke_color", stroke_color.c_str(),
								"end-arrow", TRUE,
								NULL);

						g_object_set_data (G_OBJECT (dmLines.imgPoints[thisInd].line_next), 
								"this_ind", GINT_TO_POINTER(thisInd));
						g_object_set_data (G_OBJECT (dmLines.imgPoints[thisInd].line_next), 
								"next_ind", GINT_TO_POINTER(nextInd));

						// fill in previous line
						int p_ind = 0;
						for( ; p_ind < MAX_PREV_LINE; p_ind++) {
							if( !dmLines.imgPoints[nextInd].line_prev[p_ind] ) {
								dmLines.imgPoints[nextInd].line_prev[p_ind] = dmLines.imgPoints[thisInd].line_next;
								break;
							}
						}
						if( p_ind == MAX_PREV_LINE )
							cout << "FLIGHTPLAN - too many plans converge to one point" << endl;
					}
				}
				j = FindNextWPMaskBitSet(plan_mask, (uint32_t)j);
			}

			// clear these points from the mask so that we don't draw them twice
			InvertAllWPMaskBits(plan_mask);
			WPMaskAND(mask, plan_mask);
		} else
			ClearWPMaskBit(mask, i);

		i = FindNextWPMaskBitSet(mask, (uint32_t)i);
	} // Until the points are gone

	//--  loop through and draw the waypoints points --//
	for( int i = 0; i < NUM_WP_POINTS; i++ ) {
		// get the point
		GPSWaypoint_t gpsPt = dmLines.plan.get_waypoint(i);
		int ind = gpsPt.index;
		if( ind < NUM_WP_POINTS ) {
			// Create group to hold waypoint and text
			dmLines.imgPoints[ind].pointG = goo_canvas_group_new (dmLines.wp_group,
					"x", dmLines.imgPoints[ind].x,
					"y", dmLines.imgPoints[ind].y,
					NULL);
			setup_item(GOO_CANVAS_ITEM(dmLines.imgPoints[ind].pointG));

			// draw circle for orbit point 
			if( gpsPt.OrbitRadius > 0 )
			{
				double delta_pix  = gpsPt.OrbitRadius / gps_map->mPerP( gpsPt.Lat );
				dmLines.imgPoints[ind].orbit = goo_canvas_ellipse_new( 
						dmLines.imgPoints[ind].pointG,
						0.0, 0.0,
						delta_pix, delta_pix,
						"stroke_color", stroke_color.c_str(),
						"line-width", lineWidth,
						NULL );
			}

			// A rectangle for waypoint
			dmLines.imgPoints[ind].point = goo_canvas_rect_new( 
					dmLines.imgPoints[ind].pointG,
					-pointWidth/2.0, -pointWidth/2.0,
					pointWidth, pointWidth,
					"fill_color", "black",
					"stroke_color", "black",
					"line-width", lineWidth,
					NULL) ;

			// segment distance text
			double range = meterToLengthU(dmLines.imgPoints[ind].distPrev, guiStr.config.units);
			char *text = dmLines.imgPoints[ind].distPrev_txt;

			sprintf( text, "%.1f %s", range, lengthU_str(guiStr.config.units, range));

			dmLines.imgPoints[ind].text_p = goo_canvas_text_new (
					dmLines.imgPoints[ind].pointG,
					dmLines.imgPoints[ind].distPrev_txt, 
					-1.5*pointWidth, -1.5*pointWidth, -1,
					GTK_ANCHOR_CENTER,
					"fill_color", "blue",
					"font", "Sans 8px",
					NULL);

			// total distance text
			range = meterToLengthU(dmLines.imgPoints[ind].distTotal, guiStr.config.units);
			text = dmLines.imgPoints[ind].distTotal_txt;

			sprintf( text, "%.1f %s", range, lengthU_str(guiStr.config.units, range));

			dmLines.imgPoints[ind].text_t = goo_canvas_text_new (
					dmLines.imgPoints[ind].pointG,
					dmLines.imgPoints[ind].distTotal_txt, 
					1.5*pointWidth, 1.5*pointWidth, -1,
					GTK_ANCHOR_CENTER,
					"fill_color", "black",
					"font", "Sans 8px",
					NULL);

/*
			// text - index number
			char pointName[10];
			sprintf(pointName,"%i",ind);
			dmLines.imgPoints[ind].text = goo_canvas_text_new (
					dmLines.imgPoints[ind].pointG,
					pointName, -1.5*pointWidth, -1.5*pointWidth, -1,
					GTK_ANCHOR_CENTER,
					"fill_color", "blue",
					"font", "Sans 8px",
					NULL);
*/
		}
	}

	//cout << "DMManager::create_canvas_flightplan - bye" << endl;

}

void DMManager::setup_item(GooCanvasItem *item)
{  
	g_signal_connect (G_OBJECT (item), "button-press-event", 
			G_CALLBACK (DMManager::button_press_event_cb), this);
	g_signal_connect (G_OBJECT (item), "button-release-event", 
			G_CALLBACK (DMManager::button_release_event_cb), this);
	g_signal_connect (G_OBJECT (item), "motion-notify-event", 
			G_CALLBACK (DMManager::motion_notify_event_cb), this);
}     

bool DMManager::button_press_event_cb (GooCanvasItem *item, GooCanvasItem *target_item, GdkEventButton *event, DMManager *me)
{
/*
	cout << "DMManager::button_press_event_cb: " 
		<< " me=" << me
		<< " event->type=" << event->type 
		<<"  event->x=" <<  event->x
		<< " event->y=" <<  event->y << endl;
*/

	if( me == NULL )
		return FALSE;

	GdkCursor *pointer;

	// save mouse position, these are world coordinates
	me->mouse_x = event->x;
	me->mouse_y = event->y;

	me->drag_point = -1;

	// find the point for wich this signal came
		if( me->dmLines.wp_group ) {
			for( int pointInd = 0; pointInd < NUM_WP_POINTS; pointInd++ ) {
				if( me->dmLines.imgPoints[pointInd].pointG == item ) {
					me->drag_point = pointInd;

					pointInd = NUM_WP_POINTS;
				}
			}
		}

	if( me->drag_point == -1 ) {
		cout << " - did not find valid point group for mouse button" << endl;
		return FALSE;
	} 

	switch (event->type) {
		case GDK_2BUTTON_PRESS:
			if( me->creating_plan ) {

				int ind = me->numPoints();
				GPSWaypoint_t point = me->get_waypoint(ind-1);
/*
				if( me->drag_point == ind - 1) {
					// if we double click on final point, make it an orbit
					point.Next           = point.index - 1; // point to previous point
					point.OrbitRadius    = 100; // make 100 m orbit default
					point.OrbitDirection = 0; // make CW orbit direction
				} else {
					// close the flight plan to the point selected
					point.Next = me->drag_point;
				}
*/

				me->set_waypoint(point);

				// now that its closed, stop waypoint editing
				me->gps_map->setDistEditing( FALSE );
				me->clear_client_canvas();
				me->create_canvas_flightplan();

				// clean up mouse state
				goo_canvas_pointer_ungrab(me->gps_map->getCanvas(), item, event->time);
				me->drag_point  = -1;
				me->drag_plan   = FALSE;
				return TRUE;
			}
			break;

		case GDK_BUTTON_PRESS: 
			switch(event->button) 
			{
				case LEFT_MOUSE:

					// make sure its the local flight plan when creating
					if( !me->creating_plan ) {
						me->drag_point  = -1;
						me->drag_plan   = FALSE;
						return TRUE;
					}

					// set curser based on wether we are moving a point, moving a plan, or rotating a plan
					if(event->state & GDK_CONTROL_MASK)  {
						pointer = gdk_cursor_new(GDK_CIRCLE);
						me->drag_plan   = FALSE;
						me->rotate_plan = FALSE;
					} else if(event->state & GDK_SHIFT_MASK)  {
						pointer = gdk_cursor_new(GDK_CIRCLE);
						me->drag_plan   = FALSE;
						me->rotate_plan = TRUE;
					} else {
						pointer = gdk_cursor_new(GDK_DOTBOX);
						me->rotate_plan = FALSE;
						me->drag_plan   = TRUE;
					}

					// set dragging to true
					me->dragging = TRUE;

					// grab pointG for this point
					goo_canvas_pointer_grab(me->gps_map->getCanvas(), item,
							(GdkEventMask)(GDK_POINTER_MOTION_MASK | GDK_BUTTON_RELEASE_MASK),
							pointer,
							event->time);
					gdk_cursor_destroy(pointer);

					return TRUE;
					break;

				case RIGHT_MOUSE:
					//make sure we are not dragging anything
					me->dragging = FALSE;
					return TRUE;
					break;

				case MIDDLE_MOUSE:
					//make sure we are not dragging anything
					me->dragging = FALSE;
					break;

				default:
					printf("Unknown mouse button value\n");
					break;
			} 
		default:
			break;
	}

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return FALSE;
}

bool DMManager::button_release_event_cb(GooCanvasItem *item, GooCanvasItem *target_item, GdkEventButton *event, DMManager *me)
{
	//cout << "DMManager::button_release_event_cb" << endl;
	if( me == NULL )
		return FALSE;

	if( me->drag_point != -1 ) {
		goo_canvas_pointer_ungrab(me->gps_map->getCanvas(), item, event->time);

#ifdef EDIT_REMOTE_DM
		// need to send the command if we edited a clients flight plan
		if( me->drag_client < MAX_CLIENTS ) {
			if( !me->send_flightplan(me->drag_client, me->drag_client, me->drag_point) ) {
				cout << "Did not send flight plan" << endl;
			}
		}
#endif
	}

	me->drag_point  = -1;
	me->dragging    = FALSE;
	me->rotate_plan = FALSE;
	me->last_theta  = NAN;

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return FALSE;
}

bool DMManager::motion_notify_event_cb(GooCanvasItem *item, GooCanvasItem *target_item, GdkEventMotion *event, DMManager *me)
{
	//cout << "DMManager::motion_notify_event_cb" << endl;
	if( me == NULL )
		return FALSE;

	if( me->dragging == TRUE && me->drag_point >= 0) 
	{
		if( !me->drag_plan && !me->rotate_plan ) {
			me->translate_point(me->drag_point, event->x - me->mouse_x, event->y - me->mouse_y);

		} else {
			//-- with starting index, get mask for plan and check existence
			WPMask_t Mask;
			int numPoints = me->dmLines.plan.GetPointsInPlan(Mask, me->drag_point);
			if( numPoints <= 0 ) {
				cout << "!!! no points in waypoint plan" << endl;
				return false;
			}

			// if rotating a plan, get the angle to rotate
			double dTheta = 0;
			if( me->rotate_plan ) {
				// get the x,y img coordinate of the selected point
				double base_x, base_y;
				GooCanvasItem *baseG = me->dmLines.imgPoints[me->drag_point].pointG;
				g_object_get (G_OBJECT (baseG), "x", &base_x, "y", &base_y, NULL);

				// calc angle
				double theta = atan2( event->y - base_y, event->x - base_x);
				if( me->last_theta != me->last_theta ) me->last_theta = 0;

				// calc delta angle
				dTheta = theta - me->last_theta;

				// save value for next round
				me->last_theta = theta;
			}

			// for a flight plan, we need to find the first point, then loop through
			// the plan to update the points
			int curr_ind = me->dmLines.plan.find_first_in_plan(me->drag_point);

			//-- now loop through all points in plan
			while( GetWPMaskBit(Mask, curr_ind) )
			{
				if( me->rotate_plan ) {
					// don't rotate point we are rotating about
					if( curr_ind != me->drag_point ){
						me->rotate_point(curr_ind, dTheta);
					}
				} else
					me->translate_point(curr_ind, event->x - me->mouse_x, event->y - me->mouse_y);	

				// clear this point from the mask, and get the next
				ClearWPMaskBit(Mask, curr_ind);
				curr_ind = me->dmLines.plan.Plan[curr_ind].Next;
			}
		}

		// don't forget to save the mouse location
		me->mouse_x = event->x;
		me->mouse_y = event->y;
	}

	// Returning FALSE propagates the event to parent items;
	// returning TRUE ends event propagation. 
	return FALSE;
}

void DMManager::rotate_point(int pointInd, double dTheta)
{
	double item_x,item_y;
	double new_lon,new_lat;
	//cout << "DMManager::rotate_point - pointInd=" << pointInd << endl;

	// don't do any work if we don't have to
	if( dTheta == 0 ) return;

	// get the x,y img coordinate of the selected point
	double base_x, base_y;
	GooCanvasItem *baseG = dmLines.imgPoints[drag_point].pointG;
	g_object_get (G_OBJECT (baseG), "x", &base_x, "y", &base_y, NULL);

	// get the x,y img coordinate of the current pointG
	GooCanvasItem *pointG = dmLines.imgPoints[pointInd].pointG;
	g_object_get (G_OBJECT (pointG), "x", &item_x, "y", &item_y, NULL);

	double dX = item_x - base_x;
	double dY = item_y - base_y;

	item_x = cos(dTheta) * dX - sin(dTheta) * dY + base_x;
	item_y = sin(dTheta) * dX + cos(dTheta) * dY + base_y;

	// convert the new location to lat, lon
	new_lon = item_x;
	new_lat = item_y;
	gps_map->layerToGeo(GPSMap::DM_LAYER, new_lon, new_lat);

	// save the coordinates
	GPSWaypoint_t point = dmLines.plan.get_waypoint(pointInd);
	point.Lat = new_lat;
	point.Lon = new_lon;
	dmLines.plan.set_waypoint(point);

	// now update the canvas point and lines
	g_object_set (G_OBJECT (pointG), "x", item_x, "y", item_y, NULL);

	GooCanvasPoints *points;
	if( dmLines.imgPoints[pointInd].line_next ) {
		g_object_get (dmLines.imgPoints[pointInd].line_next, "points", &points, NULL);
		points->coords[0] = item_x;
		points->coords[1] = item_y;
		g_object_set (dmLines.imgPoints[pointInd].line_next, "points", points, NULL);
		goo_canvas_points_unref(points);
	}

	for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++) {
		if( dmLines.imgPoints[pointInd].line_prev[p_ind] ) {
			g_object_get (dmLines.imgPoints[pointInd].line_prev[p_ind], "points", &points, NULL);
			points->coords[2] = item_x;
			points->coords[3] = item_y;
			g_object_set (dmLines.imgPoints[pointInd].line_prev[p_ind], "points", points, NULL);
			goo_canvas_points_unref(points);
		}
	}
}


void DMManager::translate_point(int pointInd, double dX, double dY)
{
	double item_x,item_y;
	double new_lon,new_lat;
	//cout << "DMManager::translate_point" << endl;
	
	GooCanvasItem *pointG = dmLines.imgPoints[pointInd].pointG;

	// get the x,y img coordinate of the pointG
	g_object_get (G_OBJECT (pointG), "x", &item_x, "y", &item_y, NULL);

	// now translate them by mouse motion
	item_x += dX;
	item_y += dY;

	// convert the new location to lat, lon
	new_lon = item_x;
	new_lat = item_y;
	gps_map->layerToGeo(GPSMap::DM_LAYER, new_lon, new_lat);

	// save the coordinates
	GPSWaypoint_t point = dmLines.plan.get_waypoint(pointInd);
	point.Lat = new_lat;
	point.Lon = new_lon;
	dmLines.plan.set_waypoint(point);

	// now update the canvas point and lines
	g_object_set (G_OBJECT (pointG), "x", item_x, "y", item_y, NULL);

	GooCanvasPoints *points;
	if( dmLines.imgPoints[pointInd].line_next ) {
		g_object_get (dmLines.imgPoints[pointInd].line_next, "points", &points, NULL);
		points->coords[0] = item_x;
		points->coords[1] = item_y;
		g_object_set (dmLines.imgPoints[pointInd].line_next, "points", points, NULL);
		goo_canvas_points_unref(points);
	}

	for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++) {
		if( dmLines.imgPoints[pointInd].line_prev[p_ind] ) {
			g_object_get (dmLines.imgPoints[pointInd].line_prev[p_ind], "points", &points, NULL);
			points->coords[2] = item_x;
			points->coords[3] = item_y;
			g_object_set (dmLines.imgPoints[pointInd].line_prev[p_ind], "points", points, NULL);
			goo_canvas_points_unref(points);

		}
	}

	// now loop through plan and draw lines
	WPMask_t plan_mask;
	ClearAllWPMaskBits(plan_mask);
	(void)dmLines.plan.GetPointsInPlan(plan_mask, 0);
	int32_t j = FindFirstWPMaskBitSet(plan_mask);

	double total_range = 0;
	while(j >= 0)
	{
		GPSWaypoint_t gpsPt = dmLines.plan.get_waypoint(j);
		int ind = gpsPt.index;
		if( ind < NUM_WP_POINTS && gpsPt.IsPointValid() ) {
			int nextInd = gpsPt.Next;

			// get next waypoint
			GPSWaypoint_t gpsPt_next = dmLines.plan.get_waypoint(nextInd);

			if( gpsPt_next.index == nextInd && gpsPt_next.IsPointValid() )
			{
				GPSLLA_t gps1, gps2;
				gps1.lat = gpsPt.Lat;
				gps1.lon = gpsPt.Lon;
				gps1.alt = gpsPt.Alt;

				gps2.lat = gpsPt_next.Lat;
				gps2.lon = gpsPt_next.Lon;
				gps2.alt = gpsPt_next.Alt;

				// calculate distance
				double range = GPSRange(gps1, gps2);
				total_range += range;
				//cout << "Range to Next: " << range << endl;
				dmLines.imgPoints[nextInd].distPrev  = range;
				dmLines.imgPoints[nextInd].distTotal = total_range;

				// segment distance text
				range = meterToLengthU(dmLines.imgPoints[nextInd].distPrev, guiStr.config.units);
				char *text = dmLines.imgPoints[nextInd].distPrev_txt;

				sprintf( text, "%.1f %s", range, lengthU_str(guiStr.config.units, range));

				g_object_set(G_OBJECT(dmLines.imgPoints[nextInd].text_p), "text", 
						dmLines.imgPoints[nextInd].distPrev_txt, NULL);

				// total distance text
				range = meterToLengthU(dmLines.imgPoints[nextInd].distTotal, guiStr.config.units);
				text = dmLines.imgPoints[nextInd].distTotal_txt;

				sprintf( text, "%.1f %s", range, lengthU_str(guiStr.config.units, range));

				g_object_set(G_OBJECT(dmLines.imgPoints[nextInd].text_t), "text", 
						dmLines.imgPoints[nextInd].distTotal_txt, NULL);
			}
		}
		j = FindNextWPMaskBitSet(plan_mask, (uint32_t)j);
	}
}

void DMManager::clear_client()
{
	//cout << "DMManager::clear_client(int clientInd)" << endl;

	clear_client_canvas();
	dmLines.plan.clear_list();
}

void DMManager::clear_client_canvas()
{
	//cout << "DMManager::clear_client_canvas" << endl;

	for( int i=0; i < NUM_WP_POINTS; i++ ) {

		// by removing all next lines, we are removing all previous line items
		if( GOO_IS_CANVAS_ITEM(dmLines.imgPoints[i].line_next ) )
			goo_canvas_item_remove(dmLines.imgPoints[i].line_next);
		dmLines.imgPoints[i].line_next = NULL;

		// by removing the point group, we are removing the text, orbit, and point items
		// as well.
		if( GOO_IS_CANVAS_ITEM(dmLines.imgPoints[i].pointG ) )
			goo_canvas_item_remove(dmLines.imgPoints[i].pointG);
		dmLines.imgPoints[i].pointG = NULL;

		dmLines.imgPoints[i].point = NULL;
		dmLines.imgPoints[i].text  = NULL;
		dmLines.imgPoints[i].orbit = NULL;
		for( int p_ind = 0; p_ind < MAX_PREV_LINE; p_ind++ ) {
			dmLines.imgPoints[i].line_prev[p_ind] = NULL;
		}

	}
}
